/***************************************************************************
 创建者: 华磊
 开始时间: 2020.8.14
 copyright: (C) 深圳市华友高科有限公司
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************/
#include "mapscene.h"
#include <QKeyEvent>
#include <qgraphicssceneevent.h>
#include <qdebug.h>
#include <QGraphicsTextItem>
#include <QGraphicsLineItem>
#include "maplogic.h"
#include <math.h>
#include "basemapimage.h"

MapScene::MapScene(MapLogic *mapLogicIn, QObject *parent) :
    QGraphicsScene(parent)
{
    mapLogic=mapLogicIn;
    pressed_x=0;
    pressed_y=0;
    released_x=0;
    released_y=0;

    userPoint_line=NULL;
    userPoint_text=NULL;

    target_position_line=NULL;
    target_position_connect_line=NULL;
    target_position_circle=NULL;
    target_position_text=NULL;

    target_Point = NULL;
    tmp_targetPointText = NULL;

    baseMap=new BaseMapImage();
    addItem(baseMap);

    initialCommonCar();
    initialReflectCar();
    initialLaserPoint();
    initialVmark();
    initialTargetPoint();
    //    initialPoint();
    initialPathPoint();
}

int MapScene::setShowCommonCar(bool isShow)
{
    isShowCommonCar=isShow;
    return 1;
}

int MapScene::setShowCommonPositionCertainty(bool isShow)
{
    isShowCommonCertainCircle=isShow;
    return 1;
}

int MapScene::setShowReflectCar(bool isShow)
{
    isShowReflectCar=isShow;
    return 1;
}

int MapScene::setShowReflectPositionCertainty(bool isShow)
{
    isShowReflectCertainCircle=isShow;
    return 1;
}

int MapScene::freeDPointListItem()
{
    if(!vehiclePointList.isEmpty())
    {
        for(int i = 0; i < vehiclePointList.size(); i++)
        {
            // 删除图像项
            removeItem(vehiclePointList.at(i));
            delete vehiclePointList.at(i);
        }
        // 释放完后清除容器,否则会非法访问内存,造成程序崩溃
        vehiclePointList.clear();
    }

    // 删除点位索引Text项(重新打开文件时删除)
    if(!vehiclePointTextList.isEmpty())
    {
        for(int i=0; i < vehiclePointTextList.size(); i++)
        {
            removeItem(vehiclePointTextList.at(i));
            delete vehiclePointTextList.at(i);//no add
        }

        vehiclePointTextList.clear();
    }
    return 1;

}

int MapScene::reloadMap(MapPropertyInfo infoIn)
{
//    qDebug()<<mapPath;
//     QString tmpMapPgm=mapPath;
//     tmpMapPgm.remove(".yaml");
//     qDebug()<<tmpMapPgm;
//     tmpMapPgm=tmpMapPgm+".pgm";

     if(1!=mapLogic->reloadMap(infoIn))
     {
         return -1;
     }
     if(1!=baseMap->reloadMap(mapLogic->getMapPgmFilePath()))
     {
         return -1;
     }
     setSceneRect(0,0, mapLogic->getMapWidth(),mapLogic->getMapHeight());
     update();
     return 1;
}

int MapScene::getScenePosition(double &x1, double &y1, double &x2, double &y2)
{
    x1=pressed_x;
    x2=released_x;
    y1=pressed_y;
    y2=released_y;
    return 1;
}

int MapScene::getWorldPosition(double &x, double &y, double &a)
{

    a=atan2(released_y-pressed_y,released_x-pressed_x);
    return mapLogic->translateScenePoint2WorldPoint(pressed_x,pressed_y,a,x,y,a);

}

double MapScene::getWorldLength()
{
    double wx1,wx2,wy1,wy2,wr1,wr2;
    mapLogic->translateScenePoint2WorldPoint(pressed_x,pressed_y,0,wx1,wy1,wr1);
    mapLogic->translateScenePoint2WorldPoint(released_x,released_y,0,wx2,wy2,wr2);
    return sqrt((wx1-wx2)*(wx1-wx2)+(wy1-wy2)*(wy1-wy2));
}

int MapScene::setCarBorderRect(double x, double y, double width, double height)
{
    carBorder->setRect(QRectF(x,y,width,height));
    reflectcarBorder->setRect(QRectF(x,y,width,height));
    return 1;
}

int MapScene::drawTargetPoint(bool isShow,double x_pix, double y_pix, double a_degree)
{
    if(true==isShow)
    {
        QMatrix tmpMatrixIn;
        mapLogic->calculateCarTransMatrix(x_pix,y_pix,a_degree,tmpMatrixIn);
        target_position_text->setMatrix(tmpMatrixIn);
        target_position_text->show();
    }
    else
    {
        target_position_text->hide();
    }


    return 1;
//    if(NULL!=userPoint_line)
//    {
//        removeItem(userPoint_line);
//        removeItem(userPoint_text);
//        removeItem(userPoint_circle);
//    }
//    double circleRadius=6;
//    userPoint_circle=addEllipse(x1-circleRadius/2,y1-circleRadius/2,circleRadius,circleRadius);
//    userPoint_line = addLine(x1,y1, x2,y2);
//    QFont tmpFont;
//    tmpFont.setPointSize(10);
//    userPoint_text=addText("P",tmpFont);
//    userPoint_text->setPos(x1,y1);

//    QPen pen;
//    pen.setWidth(1);
//    pen.setColor(QColor(100,50,50));
//    userPoint_circle->setPen(pen);
//    userPoint_line->setPen(pen);
//    return ;
//    //initial target position
//    carBorder = new QGraphicsRectItem(QRectF(0,0,mapLogic->getCarWidth(),mapLogic->getCarHeight()));
//    QPen pen;
//    pen.setWidth(1);
//    pen.setColor(QColor(100,200,50));
//    carBorder->setPen(pen);
//    carBorder->setBrush(QColor(10,200,100));
//    addItem(carBorder);
//    carBorder->setPos(0, 0);

//    QFont tmpFont;
//    tmpFont.setPointSize(18);
//    car_text=addText("A",tmpFont);
//    car_text->setPos(0,0);

}

int MapScene::drawOneBezier(VehiclePosition startPosition, VehiclePosition targetPosition, double lengthRatio,bool isPositiveRun)
{
    currentMovingPath_line->hide();
    currentMovingPath_rotate->hide();

    std::vector<POINT> pointsOut_pix;
    calculateBezierPoints(startPosition,targetPosition,lengthRatio,isPositiveRun,pointsOut_pix);
    drawPathPoint(pointsOut_pix);
    return 1;
}

int MapScene::drawOneBezierByControlPoints(VehiclePosition startPosition, VehiclePosition targetPosition,
                                           VehiclePosition middlePose1, VehiclePosition middlePose2)
{
    currentMovingPath_line->hide();
    currentMovingPath_rotate->hide();

    std::vector<POINT> pointsOut_pix;
    calculateBezierPointsByControlPoints(startPosition,targetPosition,middlePose1,middlePose2,pointsOut_pix);
    drawPathPoint(pointsOut_pix);
    return 1;
}

int MapScene::calculateBezierPoints(VehiclePosition startPosition, VehiclePosition targetPosition, double lengthRatio,
                                    bool isPositiveRun, std::vector<POINT> &pointsOut_pix)
{

    if(false==isPositiveRun)
    {
        startPosition.rotate+=M_PI;
        targetPosition.rotate+=M_PI;
    }

    POINT p0, p1,  p2,  p3;

    p0.x=startPosition.x;
    p0.y=startPosition.y;

    p3.x=targetPosition.x;
    p3.y=targetPosition.y;

    double totalLength=sqrt((p0.x-p3.x)*(p0.x-p3.x)+(p0.y-p3.y)*(p0.y-p3.y));
    double plength=lengthRatio*totalLength;

    p1.x=p0.x+plength*cos(startPosition.rotate);
    p1.y=p0.y+plength*sin(startPosition.rotate);

    p2.x=p3.x+plength*cos(targetPosition.rotate+M_PI);//
//    qDebug()<<r2*M_PI/180.0<<cos(r2*M_PI/180.0)<<p3.x<<p2.x;
    p2.y=p3.y+plength*sin(targetPosition.rotate+M_PI);
    Bezier tmpBeizer;
    std::vector<POINT> pointsOut;

//    qDebug()<<tmpBeizer.getBezier_length(p0,p1,p2,p3,50);

    tmpBeizer.getBezierPoints(p0,p1,p2,p3,50,pointsOut);
    POINT tmpPoint;
    double tmp_a=0,tmp_b;
    for(int i=0;i<pointsOut.size();i++)
    {
        mapLogic->translateWorldPoint2ScenePoint(pointsOut[i].x,pointsOut[i].y,tmp_a,tmpPoint.x,tmpPoint.y,tmp_b);
        pointsOut_pix.push_back(tmpPoint);
    }
    return 1;
}

int MapScene::calculateBezierPointsByControlPoints(VehiclePosition startPosition, VehiclePosition targetPosition,
                                                   VehiclePosition middlePose1, VehiclePosition middlePose2,
                                                   std::vector<POINT> &pointsOut_pix)
{

    POINT p0, p1,  p2,  p3;

    p0.x=startPosition.x;
    p0.y=startPosition.y;

    p3.x=targetPosition.x;
    p3.y=targetPosition.y;

//    double totalLength=sqrt((p0.x-p3.x)*(p0.x-p3.x)+(p0.y-p3.y)*(p0.y-p3.y));
//    double plength=lengthRatio*totalLength;

    p1.x=middlePose1.x;
    p1.y=middlePose1.y;

    p2.x=middlePose2.x;//
//    qDebug()<<r2*M_PI/180.0<<cos(r2*M_PI/180.0)<<p3.x<<p2.x;
    p2.y=middlePose2.y;
    Bezier tmpBeizer;
    std::vector<POINT> pointsOut;

//    qDebug()<<tmpBeizer.getBezier_length(p0,p1,p2,p3,50);

    tmpBeizer.getBezierPoints(p0,p1,p2,p3,50,pointsOut);
    POINT tmpPoint;
    double tmp_a=0,tmp_b;
    for(int i=0;i<pointsOut.size();i++)
    {
        mapLogic->translateWorldPoint2ScenePoint(pointsOut[i].x,pointsOut[i].y,tmp_a,tmpPoint.x,tmpPoint.y,tmp_b);
        pointsOut_pix.push_back(tmpPoint);
    }
    return 1;
}

int MapScene::drawContinuousBezier(VehiclePosition startPosition, std::vector<VehiclePosition> targetPosition,
                                   double lengthRatio, bool isPositiveRun)
{
    currentMovingPath_line->hide();
    currentMovingPath_rotate->hide();

    VehiclePosition startPos;
    std::vector<POINT> pointsOut_pix;
    for(int k=0;k<targetPosition.size();k++)
    {
        if(0==k)
        {
            startPos=startPosition;

        }
        else
        {
            startPos=targetPosition[k-1];
        }
            calculateBezierPoints(startPos,targetPosition[k],lengthRatio,isPositiveRun,pointsOut_pix);
    }

    drawPathPoint(pointsOut_pix);
    return 1;
}

int MapScene::drawCurrentPath_line(VehiclePosition startPosition,VehiclePosition targetPosition)
{
    currentMovingPath_spline->hide();
    currentMovingPath_rotate->hide();
    double tmp_a,tmp_b;
    mapLogic->translateWorldPoint2ScenePoint(startPosition.x,startPosition.y,tmp_a,startPosition.x,startPosition.y,tmp_b);
    mapLogic->translateWorldPoint2ScenePoint(targetPosition.x,targetPosition.y,tmp_a,targetPosition.x,targetPosition.y,tmp_b);
    currentMovingPath_line->setLine(startPosition.x,startPosition.y,targetPosition.x,targetPosition.y);
    currentMovingPath_line->show();
    return 1;


}

int MapScene::drawCurrentPath_rotate(VehiclePosition targetPosition)
{
    currentMovingPath_line->hide();
    currentMovingPath_spline->hide();
    double tmp_a,tmp_b;
    mapLogic->translateWorldPoint2ScenePoint(targetPosition.x,targetPosition.y,tmp_a,targetPosition.x,targetPosition.y,tmp_b);
    currentMovingPath_rotate->setPos(targetPosition.x,targetPosition.y);
    currentMovingPath_rotate->show();

    return 1;
}

int MapScene::displayAllReferenceMarks(const std::vector<ReflectMarkInfo> &allReflectMarksIn)
{
    freeAllReferenceMarks();

    double circleRadius=5;
    QFont tmpFont;
    tmpFont.setPointSize(10);
    QPen pen;
    pen.setWidth(1);
    QColor tmpColor(180,150,50);
    pen.setColor(tmpColor);
    QGraphicsEllipseItem *tmpEllipseItem;
    QGraphicsTextItem *tmpTextItem;

    double tmp_x,tmp_y,tmp_a;
    for(int i=0;i<allReflectMarksIn.size();i++)
    {
        mapLogic->translateWorldPoint2ScenePoint(allReflectMarksIn[i].pose_x,
                                                 allReflectMarksIn[i].pose_y,
                                                 allReflectMarksIn[i].pose_r,
                                                 tmp_x,tmp_y,tmp_a);

        tmpEllipseItem=addEllipse(tmp_x-circleRadius/2,tmp_y-circleRadius/2,circleRadius,circleRadius);
        tmpEllipseItem->setPen(pen);
        allReferenceMarksList_dot.append(tmpEllipseItem);
        QString tmpText="R";
        tmpText+=QString::number(allReflectMarksIn[i].uniqId);
        tmpTextItem=addText(tmpText,tmpFont);
        tmpTextItem->setPos(tmp_x,tmp_y);
        tmpTextItem->setDefaultTextColor(tmpColor);
        allReferenceMarksList_text.append(tmpTextItem);

    }

    return 1;

}

int MapScene::freeAllReferenceMarks()
{
//    clear();
    if(!allReferenceMarksList_dot.isEmpty())
    {
        for(int i = 0; i < allReferenceMarksList_dot.size(); i++)
        {
            // 删除图像项
            removeItem(allReferenceMarksList_dot.at(i));
            delete allReferenceMarksList_dot.at(i);
        }
        // 释放完后清除容器,否则会非法访问内存,造成程序崩溃
        allReferenceMarksList_dot.clear();
    }

    // 删除点位索引Text项(重新打开文件时删除)
    if(!allReferenceMarksList_text.isEmpty())
    {
        for(int i=0; i < allReferenceMarksList_text.size(); i++)
        {
            removeItem(allReferenceMarksList_text.at(i));
            delete allReferenceMarksList_text.at(i);//no add
        }

        allReferenceMarksList_text.clear();
    }
    return 1;
}

int MapScene::displayCalculatedReferenceMarks(const std::vector<ReflectMarkInfo> &marksIn)
{
    freeCalculatedReferenceMarks();

    double circleRadius=5;
    QFont tmpFont;
    tmpFont.setPointSize(10);
    QPen pen;
    pen.setWidth(1);
    QColor tmpColor(180,50,150);
    pen.setColor(tmpColor);
    QGraphicsEllipseItem *tmpEllipseItem;
    QGraphicsTextItem *tmpTextItem;

    double tmp_x,tmp_y,tmp_a;
    for(int i=0;i<marksIn.size();i++)
    {
        mapLogic->translateWorldPoint2ScenePoint(marksIn[i].pose_x,
                                                 marksIn[i].pose_y,
                                                 marksIn[i].pose_r,
                                                 tmp_x,tmp_y,tmp_a);

        tmpEllipseItem=addEllipse(tmp_x-circleRadius/2,tmp_y-circleRadius/2,circleRadius,circleRadius);
        tmpEllipseItem->setPen(pen);
        calculatedMarksList_dot.append(tmpEllipseItem);
        QString tmpText="C";
        tmpText+=QString::number(marksIn[i].uniqId);
        tmpTextItem=addText(tmpText,tmpFont);
        tmpTextItem->setPos(tmp_x,tmp_y);
        tmpTextItem->setDefaultTextColor(tmpColor);
        calculatedMarksList_text.append(tmpTextItem);

    }

    return 1;

}

int MapScene::freeCalculatedReferenceMarks()
{
//    clear();
    if(!calculatedMarksList_dot.isEmpty())
    {
        for(int i = 0; i < calculatedMarksList_dot.size(); i++)
        {
            // 删除图像项
            removeItem(calculatedMarksList_dot.at(i));
            delete calculatedMarksList_dot.at(i);
        }
        // 释放完后清除容器,否则会非法访问内存,造成程序崩溃
        calculatedMarksList_dot.clear();
    }

    // 删除点位索引Text项(重新打开文件时删除)
    if(!calculatedMarksList_text.isEmpty())
    {
        for(int i=0; i < calculatedMarksList_text.size(); i++)
        {
            removeItem(calculatedMarksList_text.at(i));
            delete calculatedMarksList_text.at(i);//no add
        }

        calculatedMarksList_text.clear();
    }
    return 1;
}

int MapScene::displayDetectedLaserPoints(std::vector<LaserMarkInfo> laserMarks)
{
    freeDetectedLaserPoints();

    double circleRadius=5;
    QFont tmpFont;
    tmpFont.setPointSize(10);
    QPen pen;
    pen.setWidth(1);
    QColor tmpColor(80,150,190);
    pen.setColor(tmpColor);
    QGraphicsEllipseItem *tmpEllipseItem;
    QGraphicsTextItem *tmpTextItem;

    double tmp_x,tmp_y,tmp_a;
    for(int i=0;i<laserMarks.size();i++)
    {
        mapLogic->translateWorldPoint2ScenePoint(laserMarks[i].pose_x,
                                                 laserMarks[i].pose_y,
                                                 laserMarks[i].pose_r,
                                                 tmp_x,tmp_y,tmp_a);

        tmpEllipseItem=addEllipse(tmp_x-circleRadius/2,tmp_y-circleRadius/2,circleRadius,circleRadius);
        tmpEllipseItem->setPen(pen);
        detectedLaserList_dot.append(tmpEllipseItem);
        QString tmpText="L";
        tmpText+=QString::number(i);
        tmpTextItem=addText(tmpText,tmpFont);
        tmpTextItem->setPos(tmp_x,tmp_y);
        tmpTextItem->setDefaultTextColor(tmpColor);
        detectedLaserList_text.append(tmpTextItem);

    }

    return 1;

}

int MapScene::freeDetectedLaserPoints()
{
//    clear();
    if(!detectedLaserList_dot.isEmpty())
    {
        for(int i = 0; i < detectedLaserList_dot.size(); i++)
        {
            // 删除图像项
            removeItem(detectedLaserList_dot.at(i));
            delete detectedLaserList_dot.at(i);
        }
        // 释放完后清除容器,否则会非法访问内存,造成程序崩溃
        detectedLaserList_dot.clear();
    }

    // 删除点位索引Text项(重新打开文件时删除)
    if(!detectedLaserList_text.isEmpty())
    {
        for(int i=0; i < detectedLaserList_text.size(); i++)
        {
            removeItem(detectedLaserList_text.at(i));
            delete detectedLaserList_text.at(i);//no add
        }

        detectedLaserList_text.clear();
    }
    return 1;



}


int MapScene::initialCommonCar()
{
    isShowCommonCar=true;
    isShowCommonCertainCircle=true;

//    std::vector<struct VehiclePosition> carBorderOffsetOut;
//    mapDelegate->getCarBorderOffset(carBorderOffsetOut);
//    mapLogic->setCarBorderAndOffset(carBorderOffsetOut);

    carBorder = new QGraphicsRectItem(QRectF(0,0,mapLogic->getCarWidth(),mapLogic->getCarHeight()));
    QPen pen;
    pen.setWidth(1);
    pen.setColor(QColor(100,200,50));
    carBorder->setPen(pen);
    carBorder->setBrush(QColor(10,200,100));
    addItem(carBorder);
    carBorder->setPos(0, 0);

    QFont tmpFont;
    tmpFont.setPointSize(18);
    car_text=addText("A",tmpFont);
    car_text->setPos(0,0);
    car_text->setDefaultTextColor(QColor(0,0,150));

    car_positionCertainCircle=NULL;


    return 1;
}

int MapScene::initialReflectCar()
{
    isShowReflectCar=false;
    isShowReflectCertainCircle=false;

    reflectcarBorder = new QGraphicsRectItem(QRectF(0,0,mapLogic->getCarWidth(),mapLogic->getCarHeight()));
    QPen pen;
    pen.setWidth(1);
    pen.setColor(QColor(10,20,100));
    reflectcarBorder->setPen(pen);
    reflectcarBorder->setBrush(QColor(10,20,100));
    addItem(reflectcarBorder);
    reflectcarBorder->setPos(0, 0);

    QFont tmpFont;
    tmpFont.setPointSize(18);
    reflectcar_text=addText("B",tmpFont);
    reflectcar_text->setPos(0,0);
    reflectcar_text->setDefaultTextColor(QColor(200,0,150));

    reflectcar_positionCertainCircle=NULL;
//    reflectcar_positionCertainCircle=addEllipse(tmp_x-circleRadius/2,tmp_y-circleRadius/2,circleRadius,circleRadius);
//    reflectcar_positionCertainCircle->setPen(pen);
}

int MapScene::initialTargetPoint()
{
    QFont tmpFont;
    tmpFont.setPointSize(18);
    target_position_text=addText("A目标",tmpFont);
    target_position_text->setPos(0,0);
    target_position_text->setDefaultTextColor(QColor(0,150,100));
    target_position_text->hide();
}

int MapScene::initialVmark()
{
    QFont tmpFont;
    tmpFont.setPointSize(10);
    vmark_text=addText("V槽探测到",tmpFont);
    vmark_text->setPos(0,0);
    vmark_text->setDefaultTextColor(QColor(200,80,0));
    vmark_text->hide();


}

int MapScene::initialLaserPoint()
{
    laserPointPolygon = new QGraphicsPolygonItem();
    QPointF tmpPointF1(0,0);
    QPointF tmpPointF2(0,100);
    QPointF tmpPointF3(100,100);
    QPointF tmpPointF4(-100,-100);
    QVector<QPointF> tmpPointFVector;
    tmpPointFVector.append(tmpPointF1);
    tmpPointFVector.append(tmpPointF2);
    tmpPointFVector.append(tmpPointF3);
    tmpPointFVector.append(tmpPointF4);
    QPolygonF tmpPolygon(tmpPointFVector);
    laserPointPolygon->setPolygon(tmpPolygon);

    QPen pen;
//    pen.setWidth(1);
    pen.setColor(QColor(255,0,0));
    laserPointPolygon->setPen(pen);
//    laserPointPolygon->setBrush(QColor(10,200,100));
    addItem(laserPointPolygon);


    return 1;


}

int MapScene::initialPathPoint()
{
    //样条曲线初始化
    currentMovingPath_spline = new QGraphicsPolygonItem();
    QPointF tmpPointF1(0,0);
    QPointF tmpPointF2(0,100);
    QPointF tmpPointF3(100,100);
    QPointF tmpPointF4(-100,-100);
    QVector<QPointF> tmpPointFVector;
    tmpPointFVector.append(tmpPointF1);
    tmpPointFVector.append(tmpPointF2);
    tmpPointFVector.append(tmpPointF3);
    tmpPointFVector.append(tmpPointF4);
    QPolygonF tmpPolygon(tmpPointFVector);
    currentMovingPath_spline->setPolygon(tmpPolygon);

    QPen pen;
    pen.setWidth(1);
    pen.setColor(QColor(0,150,200));
    currentMovingPath_spline->setPen(pen);
//    laserPointPolygon->setBrush(QColor(10,200,100));
    addItem(currentMovingPath_spline);
    currentMovingPath_spline->hide();

    //直线初始化
    currentMovingPath_line=new QGraphicsLineItem;
    currentMovingPath_line->setPen(pen);
    addItem(currentMovingPath_line);
    currentMovingPath_line->hide();

    //旋转初始化
    QFont tmpFont;
    tmpFont.setPointSize(10);
    currentMovingPath_rotate=addText("旋转",tmpFont);
    currentMovingPath_rotate->setPos(0,0);
    currentMovingPath_rotate->setDefaultTextColor(QColor(0,150,200));
    currentMovingPath_rotate->hide();

    return 1;
}

int MapScene::initialPoint()
{
    /******* 点位图形绘制 *********/
    // 设置可选中　(优化:选中后显示当前点位信息
    QPixmap image("/agvwork/AgvHmi/baseBag/hmiProject/resources/mapView/direction.png");
    tmp_target_Point = new QGraphicsPixmapItem();
//    tmpPointList.append(tmp_target_Point);
    // 设置图片缩放
    tmp_target_Point->setPixmap(image.scaled(20, 20));
    // 设置可选中　(优化:选中后显示当前点位信息)
    tmp_target_Point->setFlags(QGraphicsItem::ItemIsSelectable);

    addItem(tmp_target_Point);
    return 1;
}

int MapScene::drawCommonCar(double x_pix, double y_pix, double a_degree)
{

    car_positionX_P=x_pix;//PIX
    car_positionY_P=y_pix;//PIX
    car_positionA_P=a_degree;//degree
    if(NULL!=carBorder)
    {
        QMatrix tmpMatrixIn;
        mapLogic->calculateCarTransMatrix(x_pix,y_pix,a_degree,tmpMatrixIn);
        carBorder->setMatrix(tmpMatrixIn);

        car_text->setMatrix(tmpMatrixIn);

        if(isShowCommonCar)
        {
            carBorder->show();
            car_text->show();
        }
        else
        {
            carBorder->hide();
            car_text->hide();

        }

    }

    return 1;

}

int MapScene::drawCommonPositionCertainty(double x, double y, double positionError)
{
    if(NULL!=car_positionCertainCircle)
    {
        removeItem(car_positionCertainCircle);
        delete car_positionCertainCircle;
    }
    double circleRadius=fabs(positionError);
    double tmp_x,tmp_y,tmp_a;
    mapLogic->translateWorldPoint2ScenePoint(x,y,0,tmp_x,tmp_y,tmp_a);

    car_positionCertainCircle=addEllipse(tmp_x-circleRadius/2,tmp_y-circleRadius/2,circleRadius,circleRadius);
    car_positionCertainCircle->setPen(QColor(100,200,50));

    if(isShowCommonCertainCircle)
    {
        car_positionCertainCircle->show();
    }
    else
    {
        car_positionCertainCircle->hide();

    }
    return 1;
}

int MapScene::drawReflectCar(double x_pix, double y_pix, double a_degree)
{

    if(NULL!=reflectcarBorder)
    {
        QMatrix tmpMatrixIn;
        mapLogic->calculateCarTransMatrix(x_pix,y_pix,a_degree,tmpMatrixIn);
        reflectcarBorder->setMatrix(tmpMatrixIn);

        reflectcar_text->setMatrix(tmpMatrixIn);

        if(isShowReflectCar)
        {
            reflectcarBorder->show();
            reflectcar_text->show();
        }
        else
        {
            reflectcarBorder->hide();
            reflectcar_text->hide();

        }

    }

    return 1;
}

int MapScene::drawReflectPositionCertainty(double x, double y, double positionError)
{
    if(NULL!=reflectcar_positionCertainCircle)
    {
        removeItem(reflectcar_positionCertainCircle);
        delete reflectcar_positionCertainCircle;
    }
    double circleRadius=fabs(positionError);
    double tmp_x,tmp_y,tmp_a;
    mapLogic->translateWorldPoint2ScenePoint(x,y,0,tmp_x,tmp_y,tmp_a);

    reflectcar_positionCertainCircle=addEllipse(tmp_x-circleRadius/2,tmp_y-circleRadius/2,circleRadius,circleRadius);
    reflectcar_positionCertainCircle->setPen(QColor(10,20,100));

    if(isShowReflectCertainCircle)
    {
        reflectcar_positionCertainCircle->show();
    }
    else
    {
        reflectcar_positionCertainCircle->hide();

    }
    return 1;
}

int MapScene::drawVmark(bool isShow,double x_pix, double y_pix, double a_degree)
{
    if(true==isShow)
    {
        QMatrix tmpMatrixIn;
        mapLogic->calculateCarTransMatrix(x_pix,y_pix,a_degree,tmpMatrixIn);
        vmark_text->setMatrix(tmpMatrixIn);
        vmark_text->show();
    }
    else
    {
        vmark_text->hide();
    }


    return 1;
}

int MapScene::drawLaserPoint(std::vector<PointCloudData_3D> &pointCloudOut, double x, double y, double a)
{
    //注意屏幕坐标系是左手坐标系，ｒｏｓ里面时右手坐标系
    if(pointCloudOut.size()<=0)
    {
        return -1;
    }

    QPointF tmpPointF1(0,0);
    QVector<QPointF> tmpPointFVector;

    int laserCount=pointCloudOut.size();
    for(int laserIndex=0;laserIndex<laserCount;laserIndex++)
    {
        int pointCount=pointCloudOut[laserIndex].point_x.size();
        for(int i=0;i<pointCount;i++)
        {
            tmpPointF1.setX(pointCloudOut[laserIndex].point_x[i]/mapLogic->getMapResolution());
            tmpPointF1.setY(-pointCloudOut[laserIndex].point_y[i]/mapLogic->getMapResolution());
            tmpPointFVector.append(tmpPointF1);
        }
    }
    QPolygonF tmpPolygon(tmpPointFVector);
    laserPointPolygon->setPolygon(tmpPolygon);

    if(NULL!=laserPointPolygon)
    {
        QMatrix tmpMatrixIn;
        double x_out,y_out, a_out;
//        mapLogic->translateWorldPoint2ScenePoint_noBias(car_positionX_M,car_positionY_M,car_positionA_M
//                                                        ,x_out,y_out, a_out);
//        mapLogic->calculateLaserTransMatrix(x_out,y_out, a_out,tmpMatrixIn);
        mapLogic->calculateLaserTransMatrix(car_positionX_P,car_positionY_P, (car_positionA_P-90),tmpMatrixIn);
        laserPointPolygon->setMatrix(tmpMatrixIn);
    }
    laserPointPolygon->show();
    return 1;

}

int MapScene::drawPathPoint(const std::vector<POINT> &pointList)
{
    //注意屏幕坐标系是左手坐标系，ｒｏｓ里面时右手坐标系
    if(pointList.size()<=0)
    {
        return -1;
    }

    QPointF tmpPointF1(0,0);
    QVector<QPointF> tmpPointFVector;


        for(int i=0;i<pointList.size();i++)
        {
            tmpPointF1.setX(pointList[i].x);
            tmpPointF1.setY(pointList[i].y);
            tmpPointFVector.append(tmpPointF1);
        }

    QPolygonF tmpPolygon(tmpPointFVector);
    currentMovingPath_spline->setPolygon(tmpPolygon);


    currentMovingPath_spline->show();
    return 1;
}

int MapScene::hideLaserPoint()
{
    laserPointPolygon->hide();
    return 1;

}

int MapScene::drawVehiclePointList(bool isShow, double pos_X_Pix, double pos_Y_Pix, double pos_A_Degree, int pointIndex)
{
    if(isShow)
    {
        QPixmap image("/agvwork/AgvHmi/baseBag/hmiProject/resources/mapView/direction.png");
        tmp_target_Point = new QGraphicsPixmapItem();
        vehiclePointList.append(tmp_target_Point);
        addItem(tmp_target_Point);

        // 设置图片缩放
        double pictureWidth=10;
        tmp_target_Point->setPixmap(image.scaled(pictureWidth, pictureWidth));//image.scaled(10, 10)
        // 设置可选中　(优化:选中后显示当前点位信息)
//        tmp_target_Point->setFlags(QGraphicsItem::ItemIsSelectable);

        QFont tmpFont;
        tmpFont.setPointSize(10);
        // 获取格式化文本项
        tmp_targetPointText = addText(QString::number(pointIndex),tmpFont);
        tmp_targetPointText->setDefaultTextColor(QColor(0,0,200));
        vehiclePointTextList.append(tmp_targetPointText);

        QMatrix tmpMatrixIn;
        mapLogic->calculateDPointMatrix(pictureWidth,pos_X_Pix, pos_Y_Pix, pos_A_Degree, tmpMatrixIn);

        // 不同角度需要进行响应的角度偏移
        tmp_target_Point->setMatrix(tmpMatrixIn);


        //text
        mapLogic->calculateCarTransMatrix(pos_X_Pix, pos_Y_Pix, 0, tmpMatrixIn);
        tmp_targetPointText->setMatrix(tmpMatrixIn);
        tmp_targetPointText->moveBy(0,-12);


        tmp_targetPointText->show();
        tmp_target_Point->show();
    }
    else
    {
        tmp_target_Point->hide();
        tmp_targetPointText->hide();
    }

    return 1;
}



void MapScene::keyPressEvent(QKeyEvent *event)
{
    qDebug("*********MyScene::keyPressEvent***************");
    return QGraphicsScene::keyPressEvent(event);
}

void MapScene::mousePressEvent(QGraphicsSceneMouseEvent *event)
{
    qDebug("*********MyScene::mousePressEvent***************");

    // 获取事件现场位置
    QPointF scenePos=event->scenePos();
    qDebug()<<"x="<<scenePos.x()<<"y="<<scenePos.y();

    pressed_x=scenePos.x();
    pressed_y=scenePos.y();
    QGraphicsScene::mousePressEvent(event);
}

void MapScene::mouseReleaseEvent(QGraphicsSceneMouseEvent *event)
{
    qDebug("*********MyScene::mouseReleaseEvent***************");
    QPointF scenePos=event->scenePos();
    qDebug()<<"x="<<scenePos.x()<<"y="<<scenePos.y();
    released_x=scenePos.x();
    released_y=scenePos.y();

    double tmpx1,tmpx2,tmpy1,tmpy2;
    getScenePosition(tmpx1,tmpy1,tmpx2,tmpy2);
    drawUserClickedPoint(tmpx1,tmpy1,tmpx2,tmpy2);

    qDebug() << "mouseReleaseEvent tmpx1 :" << tmpx1;
    qDebug() << "mouseReleaseEvent tmpx2 :" << tmpx2;
    qDebug() << "mouseReleaseEvent tmpy1 :" << tmpy1;
    qDebug() << "mouseReleaseEvent tmpy2 :" << tmpy2;

    double x,y,a;
    getWorldPosition(x,y,a);
    qDebug()<<"world position:x="<<x<<"y="<<y<<"a="<<a;

    emit userPointClicked_signal(x,y,a);
    emit updatePointsSignal();

    QGraphicsScene::mouseReleaseEvent(event);
}


void MapScene::drawUserClickedPoint(double x1, double y1, double x2, double y2)
{
    if(NULL!=userPoint_line)
    {
        removeItem(userPoint_line);
        removeItem(userPoint_text);
        removeItem(userPoint_circle);
    }
    double circleRadius=5;
    userPoint_circle=addEllipse(x1-circleRadius/2,y1-circleRadius/2,circleRadius,circleRadius);
    userPoint_line = addLine(x1,y1, x2,y2);

    // 绘制字体
    QFont tmpFont;
    tmpFont.setPointSize(10);
    userPoint_text=addText("用户点P",tmpFont);
    userPoint_text->setPos(x1,y1);

    // 绘制地图选择点
    QPen pen;
    pen.setWidth(1);
    pen.setColor(QColor(0,150,50));
    userPoint_circle->setPen(pen);
    userPoint_line->setPen(pen);
    return ;
}

